/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Timer;
import file.Configurable;
import file.Configuration;
import file.DataRecorder;
import java.util.TimerTask;

/**
 *
 * @author Mentors Carl Burger
 */

/**
 * Class implements a line follower.
 *
 * Creates a separate thread which reads the line sensors and takes
 * care of driving the robot, rotating as-needed to follow the line
 */

public class LineFollower implements Configurable {

    private static LineFollower m_instance = null;

    private boolean m_enabled = false; //is the line follower enabled

    public static final double kDefaultPeriod = .05; // 50 ms

    private static double m_period    = kDefaultPeriod;
    private static String m_path      = "straight";
    private static double m_speed     = -0.5;
    private static double m_gain      = 0.65;
    private static int    m_proximity = 16;  // inches

    private double        decreaseTime = 5.0;
    private double        speedDecrease = .1;

    java.util.Timer m_controlLoop;

    Timer        timer;

    Drivetrain   drivetrain;

    DigitalInput leftSensor;    // digital inputs for line tracking sensors
    DigitalInput centerSensor;
    DigitalInput rightSensor;

    int previousLineSensorState = -1;

    boolean       atCross = false;            // if robot has arrived at end
    int           oldTimeInSeconds = -1;
    int           previousValue = 0;          // binary value from the previous loop

    private DataRecorder  dataRecorder;
    private MessageCenter messageCenter;
    private Configuration config;

    private class LineFollowerTask extends TimerTask {

        private LineFollower m_tracker;

        public LineFollowerTask (LineFollower tracker) {

            if (tracker == null) {
                throw new NullPointerException("Given LineFollower was null");
            }
            m_tracker = tracker;
        }

        public void run() {
            m_tracker.steer();
        }
    }


    public static LineFollower getInstance()
    {
        if (m_instance == null) {
            m_instance = new LineFollower();
        }
        return m_instance;
    }


    /**
     * Allocate a LineFollower object with the given constants for path, speed, gain, proximity, period
     * @param path:      straight | left | right
     * @param speed:     speed to drive the robot while following the line
     * @param gain:      used to rotate the robot to make steering corrections
     * @param proximity: distance (inches) from the wall to slow down
     */
    private LineFollower () {

        this.drivetrain     = Drivetrain.getInstance();

        // create the digital input objects to read from the sensors
        leftSensor   = new DigitalInput(LogomotionRobot.kLeftLineSensor);
        centerSensor = new DigitalInput(LogomotionRobot.kCenterLineSensor);
        rightSensor  = new DigitalInput(LogomotionRobot.kRightLineSensor);

        dataRecorder = DataRecorder.getInstance();

        messageCenter = MessageCenter.getInstance();

        config = Configuration.getInstance();
        config.register("LineFollower", this);

        // time the path over the line
        timer = new Timer();

        m_controlLoop = new java.util.Timer();
        m_controlLoop.schedule(new LineFollowerTask(this), 0L, (long) (m_period * 1000));
    }

    /**
     * Free the LineFollower object
     */
    protected void free() {
        m_controlLoop.cancel();
        m_controlLoop = null;
    }


    /**
     * Read the line sensors, calculate the rotation accordingly, and write drivetrain.
     * This should only be called by the LineFollowerTask and is created during initialization.
     */

    /* See http://decibel.ni.com/content/docs/DOC-14730 for Line Following Tutorial
 * L | C | R | Steer
 * ------------------------
 * 0 | 0 | 0 | Defaults Right, but is based on previous non-zero state
 * 0 | 0 | 1 | Straight
 * 0 | 1 | 0 | Left
 * 0 | 1 | 1 | Left
 * 1 | 0 | 0 | Left
 * 1 | 0 | 1 | Left
 * 1 | 1 | 0 | Left
 * 1 | 1 | 1 | Left or T
 *
 * R | C | L | Steer for right fork
 * ------------------------
 * 0 | 0 | 0 | Defaults Right, but is based on previous non-zero state
 * 0 | 0 | 1 | Straight
 * 0 | 1 | 0 | Right
 * 0 | 1 | 1 | Right
 * 1 | 0 | 0 | Right
 * 1 | 0 | 1 | Right
 * 1 | 1 | 0 | Right
 * 1 | 1 | 1 | Right or T
 *
 */

    private void steer() {
        boolean enabled;

        synchronized (this) {
            enabled = m_enabled; // take snapshot of these values...
        }

        if (enabled) {

            int binaryValue; // a single binary value of the three line tracking
                             // sensors
            double gain;     // the amount of steering correction to apply

            double powerProfile[];   // the selected power profile

            // set the straightLine and left-right variables depending on chosen path
            //boolean straightLine = ds.getDigitalIn(1);
            boolean straightLine = (m_path.equals("straight")) ? true : false;
            //powerProfile = (straightLine) ? straightProfile : forkProfile;
            double stopTime = (straightLine) ? 2.0 : 4.0; // when the robot should look for end
            //boolean goLeft = !ds.getDigitalIn(2) && !straightLine;
            boolean goLeft = (m_path.equals("right")) ? false : true;

            double time;
            double magnitude, direction, rotation;

            // loop until robot reaches "T" at end or 8 seconds has passed
            if ((time = timer.get()) < 12.0 && !atCross) {
                int timeInSeconds = (int) time;

                // read the sensors
                int leftValue   = leftSensor.get() ? 1 : 0;
                int centerValue = centerSensor.get() ? 1 : 0;
                int rightValue  = rightSensor.get() ? 1 : 0;

                // compute the single value from the 3 sensors. Notice that the bits
                // for the outside sensors are flipped depending on left or right
                // fork. Also the sign of the steering direction is different for left/right.
                if (goLeft) {
                    binaryValue = leftValue * 4 + centerValue * 2 + rightValue;
                    gain = -m_gain; // negative is left turn
                } else {
                    binaryValue = rightValue * 4 + centerValue * 2 + leftValue;
                    gain = m_gain;  // positive is right turn
                }

                //set the speed based on amount of time passed
                if(timer.get() > decreaseTime){
                    magnitude = m_speed - speedDecrease;
                }

                // get the default speed and turn rate at this time
             // magnitude = powerProfile[timeInSeconds];
                magnitude = m_speed;
                direction = 0;
                rotation = 0;

                // different cases for different line tracking sensor readings
                switch (binaryValue) {
                    case 1:  // on line edge
                        rotation = 0;
                        break;

                    case 7:  // all sensors on (maybe at cross)
//                        if (time > stopTime) {
                            // stop moving
                            atCross = true;
                            magnitude = 0;
//                        }
                        break;

                    case 0:  // all sensors off
                        if (previousValue == 0 || previousValue == 1) {
                            rotation = gain;
                        } else {
                            rotation = -gain;
                        }
                        break;

                    default:  // all other cases
                        rotation = -gain;
                }

                // set the robot speed and direction
                drivetrain.drive(magnitude, direction, -rotation);
                
                // print current status for debugging
//                if (binaryValue != previousValue) {
//                    System.out.println("Time: " + time + " Sensor: " + binaryValue +
//                                       " speed: " + magnitude + " turn: " + rotation +
//                                       " atCross: " + atCross);
//                }

                if (binaryValue != 0) {
                    previousValue = binaryValue;
                }

                oldTimeInSeconds = timeInSeconds;


            // if at T, see if rotation needed to be perpendicular to the wall.  May need to back up to place shape

            }
        }
    }


    /**
     * Set constants for path, speed, gain, proximity, period
     * @param path:      straight | left | right
     * @param speed:     speed to drive the robot while following the line
     * @param gain:      used to rotate the robot to make steering corrections
     * @param proximity: distance (inches) from the wall to slow down
     */
    public void setControls (String path, double speed, double gain, int proximity) {

        m_path      = path;
        m_speed     = speed;
        m_gain      = gain;
        m_proximity = proximity;
    }

    /**
     * Set the LineFollower path parameter.
     * @param path is straight | left | right
     */
    public synchronized void setPath (String path) {
        m_path = path;
    }

    /**
     * Set the LineFollower speed parameter.
     * @param speed the speed to follow the line
     */
    public synchronized void setSpeed (double speed) {
        m_speed = speed;
    }

    /**
     * Set the LineFollower gain parameter.
     * @param gain is used to rotate the robot
     */
    public synchronized void setGain (double gain) {
        m_gain = gain;
    }

    /**
     * Set the LineFollower proximity parameter.
     * @param proximity is used to slow down the robot as it approaches the wall
     */
    public synchronized void setProximity (int proximity) {
        m_proximity = proximity;
    }

    /**
     * Return true if the robot has reached the T and has stopped
     * @return true if the robot is in position
     */
    public synchronized boolean onTarget() {
        return atCross;
    }

    /**
     * Begin running the LineFollower
     */
    public synchronized void enable() {
        m_enabled = true;

        previousLineSensorState = -1;
        atCross          = false;
        oldTimeInSeconds = -1;
        previousValue    = 0;

        timer.start();
        timer.reset();
    }

    /**
     * Stop running the LineFollower, this stops the robot before stopping.
     */
    public synchronized void disable() {
        //stop the robot
        m_enabled = false;
    }

    /**
     * Return true if LineFollower is enabled.
     */
    public synchronized boolean isEnable() {
        return m_enabled;
    }


    /**
     * displaySensorStatus is called from disabledPeriodic and teleopPeriodic
     * to display the line sensor values
     */
    public int getSensorStatus() {

        // read the sensors
        int leftValue   = leftSensor.get() ? 1 : 0;
        int centerValue = centerSensor.get() ? 1 : 0;
        int rightValue  = rightSensor.get() ? 1 : 0;

        return leftValue * 4 + centerValue * 2 + rightValue;
    }


    /**
     * displaySensorStatus is called from disabledPeriodic and teleopPeriodic
     * to display the line sensor values
     */
    public void outputStatus() {

        int lineSensorState = getSensorStatus();

        int left = lineSensorState >> 2;
        int center = (lineSensorState & 2) >> 1;
        int right = lineSensorState & 1;

        

        if (lineSensorState != previousLineSensorState) {

            messageCenter.println(MessageCenter.kEncodersScreen,messageCenter.kLine6,
                                  "Line sensors: " + left + "" + center + "" + right);
            previousLineSensorState = lineSensorState;
        }
    }
    
    public void recordStatus(){
        int lineSensorState = getSensorStatus();

        int left = lineSensorState >> 2;
        int center = (lineSensorState & 2) >> 1;
        int right = lineSensorState & 1;

        String status = Integer.toString(left) + Integer.toString(center) + Integer.toString(right);
        
        dataRecorder.record("lineSensors",status);
    }

    public void update(String name, String[] values) {
        if(name.equals("Steering.Gain")){
             double val = Double.parseDouble(values[0]);

             m_gain = val;
             System.out.println("Steering gain: " + val);
        }else if(name.equals("Decrease.Time")){
            decreaseTime = Double.parseDouble(values[0]);
        }else if(name.equals("Speed.Decrease")){
            speedDecrease = Double.parseDouble(values[0]);
        }
    }
}